﻿// Date: 26.07.11

// Revision 2

// This is similar to the GoblinXNA MarkerNode implementation

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using NOVA.Components.MarkerTracking;
using NOVA.Components.MarkerTracking.Vision.Marker;
using NOVA.Utilities;

namespace NOVA.Scenery
{
    /// <summary>
    /// Special controller class, which handles the transformation of a optical marker.
    /// </summary>
    public class ARMarkerController : Controller
    {
        #region Member Fields

        protected Scene m_scene;

        protected object m_markerID;
        protected int m_iMaxDropOuts;
        protected int m_iDropOut;
        protected bool m_bFound;
        protected bool m_bOptimize;
        protected ISmoother m_smoother;
        protected IPredictor m_predictor;
        protected IMarkerTracker m_tracker;
        protected Matrix m_prevMatrix;
        protected Matrix m_WorldTransformationMatrix;
        protected bool m_bSmooth;
        protected bool m_bPredict;
        protected float m_fPredictionTime;

        protected Object[] m_markerConfigs;

        #endregion

        #region Properties

        /// <summary>
        /// Gets whether the marker is detected.
        /// </summary>
        public bool MarkerFound
        {
            get { return m_bFound; }
        }

        /// <summary>
        /// If enabled, the marker transformation matrix is smoothed.
        /// </summary>
        public bool Smooth
        {
            get { return m_bSmooth; }
            set { m_bSmooth = value; }
        }

        /// <summary>
        /// Get the transformation defined by the marker.
        /// </summary>
        public Matrix WorldTransformation
        {
            get { return m_WorldTransformationMatrix; }
        }

        #endregion

        /// <summary>
        /// Special controller class, which handles the transformation of a optical marker.
        /// </summary>
        public ARMarkerController(IMarkerTracker tracker, params Object[] markerConfigs)
            : base(Vector3.Zero)
        {
            this.m_tracker = tracker;

            if (m_tracker != null)
            {
                m_markerID = m_tracker.AssociateMarker(markerConfigs);
                this.m_markerConfigs = markerConfigs;
            }

            m_bFound = false;
            m_iMaxDropOuts = 5;
            m_prevMatrix = Matrix.Identity;
            m_iDropOut = 0;
            m_bOptimize = false;

            m_smoother = null;
            m_predictor = null;
            m_bSmooth = false;
            m_bPredict = false;
            m_fPredictionTime = 0;
        }

        /// <summary>
        /// Processes the marker transformation for the current frame
        /// </summary>
        public void Update(float elapsedTime)
        {
            if (m_tracker == null && m_scene.MarkerTracker != null)
            {
                m_tracker = m_scene.MarkerTracker;
                m_markerID = m_tracker.AssociateMarker(m_markerConfigs);
            }

            if (m_tracker != null && m_tracker.FindMarker(m_markerID))
            {
                Vector3 p = new Vector3();
                Quaternion q = Quaternion.Identity;
                Matrix rawMat = m_tracker.GetMarkerTransform();

                if (m_bSmooth || m_bPredict)
                {
                    Vector3 scale;
                    rawMat.Decompose(out scale, out q, out p);
                }

                if (m_bSmooth)
                    m_smoother.FilterMatrix(ref p, ref q, out m_WorldTransformationMatrix);
                else
                    m_WorldTransformationMatrix = rawMat;

                if (m_bPredict)
                {
                    m_fPredictionTime = 0;
                    m_predictor.UpdatePredictor(ref p, ref q);
                }
                                
                m_prevMatrix = m_WorldTransformationMatrix;
                m_iDropOut = 0;
                m_bFound = true;

                Vector3 scaling, position;
                Quaternion orientation;
                rawMat.Decompose(out scaling, out orientation, out position);
                
                Orientation = orientation;

            }
            else
            {
                if (m_iMaxDropOuts < 0)
                {
                    m_WorldTransformationMatrix = m_prevMatrix;
                    m_bFound = false;
                }
                else
                {
                    if (m_iDropOut < m_iMaxDropOuts)
                    {
                        m_iDropOut++;
                        if (m_bPredict)
                        {
                            m_fPredictionTime += elapsedTime;
                            m_predictor.GetPrediction(m_fPredictionTime, out m_WorldTransformationMatrix);
                        }
                        else
                            m_WorldTransformationMatrix = m_prevMatrix;
                    }
                    else
                    {
                        m_bFound = false;
                        m_WorldTransformationMatrix = MatrixHelper.Empty;
                    }
                }

                if (m_bSmooth)
                    m_smoother.ResetHistory();
            }
        }
    }
}
